#include "Constraint.h"

#include "defs.h"

#include <iostream>
namespace jcrada {

    const int Constraint::MAX_CONNECTION_POINTS = 22;
    const double Constraint::G6DOF_MIN_ANGLE_X = -M_PI;
    const double Constraint::G6DOF_MIN_ANGLE_Y = -M_PI_2;
    const double Constraint::G6DOF_MIN_ANGLE_Z = -M_PI_2;
    const double Constraint::G6DOF_MAX_ANGLE_X = M_PI;
    const double Constraint::G6DOF_MAX_ANGLE_Y = M_PI_2;
    const double Constraint::G6DOF_MAX_ANGLE_Z = M_PI_2;
    const double Constraint::HINGE_MAX_LIMIT = 2 * M_PI;
    const double Constraint::HINGE_MIN_LIMIT = 0.0;
    const double Constraint::CONE_MIN_SWING = 0;
    const double Constraint::CONE_MAX_SWING = M_PI_2;
    const double Constraint::CONE_MIN_TWIST = 0;
    const double Constraint::CONE_MAX_TWIST = M_PI;

    Constraint::Constraint()
    :
    _id(0),
    _name(""),
    _type(CT_HINGE),
    _constraint(NULL),
    _id_body_a(0),
    _id_body_b(0),
    _connection_a(NO_CONNECTION),
    _connection_b(NO_CONNECTION),
    _hinge_lower_limit(HINGE_MIN_LIMIT),
    _hinge_upper_limit(HINGE_MIN_LIMIT),
    _linear_lower_limits(btVector3(0.0, 0.0, 0.0)),
    _linear_upper_limits(btVector3(0.0, 0.0, 0.0)),
    _angular_lower_limits(btVector3(0.0, 0.0, 0.0)),
    _angular_upper_limits(btVector3(0.0, 0.0, 0.0)),
    _cone_swing_x(CONE_MIN_SWING),
    _cone_twist_y(CONE_MIN_TWIST),
    _cone_swing_z(CONE_MIN_SWING){
    }

    Constraint::~Constraint() {

        reset();
    }

    void Constraint::set(const Creature& creature) {
        btTypedConstraint * constraint = NULL;
        switch (getType()) {
            case CT_GENERIC_6DOF:
                constraint = createGeneric6DofConstraint(creature);
                break;
            case CT_HINGE:
                constraint = createHingeConstraint(creature);
                break;
            case CT_CONE_TWIST:
                constraint = createConeTwistConstraint(creature);
                break;
            case CT_POINT_TO_POINT:
                constraint = createPointToPointConstraint(creature);
                break;
            case CT_SLIDER:
                constraint = createSliderConstraint(creature);

                break;
            default:
                BDEBUG(TO_STRING(getType()));
                assert(0);
        }
        setConstraint(constraint);
    }

    void Constraint::reset() {
        if (getConstraint()) {

            delete getConstraint();
        }
    }

    Constraint* Constraint::clone() const {
        Constraint* result = new Constraint();
        result->setAngularLowerLimits(getAngularLowerLimits());
        result->setAngularUpperLimits(getAngularUpperLimits());
        result->setConeSwingX(getConeSwingX());
        result->setConeTwistY(getConeTwistY());
        result->setConeSwingZ(getConeSwingZ());
        result->setConnectionA(getConnectionA());
        result->setConnectionB(getConnectionB());
        result->setHingeLowerLimit(getHingeLowerLimit());
        result->setHingeUpperLimit(getHingeUpperLimit());
        result->setId(getId());
        result->setIdBodyA(getIdBodyA());
        result->setIdBodyB(getIdBodyB());
        result->setLinearLowerLimits(getLinearLowerLimits());
        result->setLinearUpperLimits(getLinearUpperLimits());
        result->setName(getName());
        result->setType(getType());

        return result;
    }

    btGeneric6DofConstraint* Constraint::createGeneric6DofConstraint(const Creature& creature,
            bool use_linear_reference_frame_a) {
        btTransform local_a, local_b;
        locateConnectionOrigin(creature.getBodyPart(getIdBodyA()), getConnectionA(), local_a);
        locateConnectionOrigin(creature.getBodyPart(getIdBodyB()), getConnectionB(), local_b);

        btGeneric6DofConstraint* g6dof = new btGeneric6DofConstraint(
                *creature.getBodyPart(getIdBodyA()).getRigidBody(),
                *creature.getBodyPart(getIdBodyB()).getRigidBody(),
                local_a, local_b, use_linear_reference_frame_a);

        g6dof->setAngularLowerLimit(getAngularLowerLimits());
        g6dof->setAngularUpperLimit(getAngularUpperLimits());
        g6dof->setLinearLowerLimit(getLinearLowerLimits());
        g6dof->setLinearUpperLimit(getLinearUpperLimits());

        return g6dof;
    }

    btHingeConstraint* Constraint::createHingeConstraint(const Creature& creature,
            bool use_linear_reference_frame_a) {
        btTransform local_a, local_b;
        locateConnectionOrigin(creature.getBodyPart(getIdBodyA()), getConnectionA(), local_a);
        locateConnectionOrigin(creature.getBodyPart(getIdBodyB()), getConnectionB(), local_b);

        btHingeConstraint* result = new btHingeConstraint(
                *creature.getBodyPart(getIdBodyA()).getRigidBody(),
                *creature.getBodyPart(getIdBodyB()).getRigidBody(),
                local_a, local_b, use_linear_reference_frame_a);
        result->setLimit(getHingeLowerLimit(), getHingeUpperLimit());

        return result;
    }

    btConeTwistConstraint* Constraint::createConeTwistConstraint(const Creature& creature) {
        btTransform local_a, local_b;
        locateConnectionOrigin(creature.getBodyPart(getIdBodyA()), getConnectionA(), local_a);
        locateConnectionOrigin(creature.getBodyPart(getIdBodyB()), getConnectionB(), local_b);

        btConeTwistConstraint* result = new btConeTwistConstraint(*creature.getBodyPart(getIdBodyA()).getRigidBody(),
                *creature.getBodyPart(getIdBodyB()).getRigidBody(),
                local_a, local_b);
        
        result->setLimit(getConeSwingZ(), getConeTwistY(), getConeSwingX());

        return result;
    }

    btPoint2PointConstraint* Constraint::createPointToPointConstraint(const Creature& creature) {
        //no límite
        btTransform local_a, local_b;
        locateConnectionOrigin(creature.getBodyPart(getIdBodyA()), getConnectionA(), local_a);
        locateConnectionOrigin(creature.getBodyPart(getIdBodyB()), getConnectionB(), local_b);

        btPoint2PointConstraint* result = new btPoint2PointConstraint(*creature.getBodyPart(getIdBodyA()).getRigidBody(),
                *creature.getBodyPart(getIdBodyB()).getRigidBody(),
                local_a.getOrigin(), local_b.getOrigin());

        return result;
    }

    btSliderConstraint* Constraint::createSliderConstraint(const Creature& creature,
            bool use_linear_reference_frame_a) {
        btTransform local_a, local_b;
        locateConnectionOrigin(creature.getBodyPart(getIdBodyA()), getConnectionA(), local_a);
        locateConnectionOrigin(creature.getBodyPart(getIdBodyB()), getConnectionB(), local_b);
        btSliderConstraint* result = new btSliderConstraint(*creature.getBodyPart(getIdBodyA()).getRigidBody(),
                *creature.getBodyPart(getIdBodyB()).getRigidBody(),
                local_a, local_b, use_linear_reference_frame_a);

        return result;

    }

    void Constraint::locateConnectionOrigin(const BodyPart& body, int connection, btTransform& transform) {
        assert(connection < MAX_CONNECTION_POINTS);
        transform.setIdentity();
        switch (body.getCollisionShape()->getShapeType()) {
            case BOX_SHAPE_PROXYTYPE: locateBoxConnection(body, connection, transform);
                break;
            case SPHERE_SHAPE_PROXYTYPE: locateSphereConnection(body, connection, transform);
                break;
            case CYLINDER_SHAPE_PROXYTYPE:
            {
                btCylinderShape* shape = dynamic_cast<btCylinderShape*> (body.getCollisionShape());
                if (!shape) {
                    assert(0);
                }
                locateCylinderConnection(body, connection, shape->getUpAxis(), transform);
                break;
            }
            case CAPSULE_SHAPE_PROXYTYPE:
            {
                btCapsuleShape* shape = dynamic_cast<btCapsuleShape*> (body.getCollisionShape());
                if (!shape) {
                    assert(0);
                }
                locateCapsuleConnection(body, connection, shape->getUpAxis(), transform);
                break;
            }
            case CONE_SHAPE_PROXYTYPE:
            {
                btConeShape* shape = dynamic_cast<btConeShape*> (body.getCollisionShape());
                if (!shape) {
                    assert(0);
                }
                locateConeConnection(body, connection, shape->getConeUpIndex(), transform);

                break;
            }
            default:
                BDEBUG(TO_STRING(connection));
                assert(0);
        }
    }

    void Constraint::locateBoxConnection(const BodyPart& body, int connection, btTransform& transform) {
        btScalar x, y, z;
        switch (connection) {
                //Top Side
            case BOX_TOP:
                x = 0.0;
                y = body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            case BOX_TOP_BACK:
                x = 0.0;
                y = body.getSizeYt() / 2.0;
                z = -body.getSizeZt() / 2.0;
                break;
            case BOX_TOP_BACK_RIGHT:
                x = body.getSizeXt() / 2.0;
                y = body.getSizeYt() / 2.0;
                z = -body.getSizeZt() / 2.0;
                break;
            case BOX_TOP_RIGHT:
                x = body.getSizeXt() / 2.0;
                y = body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            case BOX_TOP_FRONT_RIGHT:
                x = body.getSizeXt() / 2.0;
                y = body.getSizeYt() / 2.0;
                z = body.getSizeZt() / 2.0;
                break;
            case BOX_TOP_FRONT:
                x = 0.0;
                y = body.getSizeYt() / 2.0;
                z = body.getSizeZt() / 2.0;
                break;
            case BOX_TOP_FRONT_LEFT:
                x = -body.getSizeXt() / 2.0;
                y = body.getSizeYt() / 2.0;
                z = body.getSizeZt() / 2.0;
                break;
            case BOX_TOP_LEFT:
                x = -body.getSizeXt() / 2.0;
                y = body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            case BOX_TOP_BACK_LEFT:
                x = -body.getSizeXt() / 2.0;
                y = body.getSizeYt() / 2.0;
                z = -body.getSizeZt() / 2.0;
                break;
                //Middle Side
            case BOX_MID_BACK:
                x = 0.0;
                y = 0.0;
                z = -body.getSizeZt() / 2.0;
                break;
            case BOX_MID_RIGHT:
                x = body.getSizeXt() / 2.0;
                y = 0.0;
                z = 0.0;
                break;
            case BOX_MID_FRONT:
                x = 0.0;
                y = 0.0;
                z = body.getSizeZt() / 2.0;
                break;
            case BOX_MID_LEFT:
                x = -body.getSizeXt() / 2.0;
                y = 0.0;
                z = 0.0;
                break;
                //Bottom Side
            case BOX_BOT_BACK:
                x = 0.0;
                y = -body.getSizeYt() / 2.0;
                z = -body.getSizeZt() / 2.0;
                break;
            case BOX_BOT_BACK_RIGHT:
                x = body.getSizeXt() / 2.0;
                y = -body.getSizeYt() / 2.0;
                z = -body.getSizeZt() / 2.0;
                break;
            case BOX_BOT_RIGHT:
                x = body.getSizeXt() / 2.0;
                y = -body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            case BOX_BOT_FRONT_RIGHT:
                x = body.getSizeXt() / 2.0;
                y = -body.getSizeYt() / 2.0;
                z = body.getSizeZt() / 2.0;
                break;
            case BOX_BOT_FRONT:
                x = 0.0;
                y = -body.getSizeYt() / 2.0;
                z = body.getSizeZt() / 2.0;
                break;
            case BOX_BOT_FRONT_LEFT:
                x = -body.getSizeXt() / 2.0;
                y = -body.getSizeYt() / 2.0;
                z = body.getSizeZt() / 2.0;
                break;
            case BOX_BOT_LEFT:
                x = -body.getSizeXt() / 2.0;
                y = -body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            case BOX_BOT_BACK_LEFT:
                x = -body.getSizeXt() / 2.0;
                y = -body.getSizeYt() / 2.0;
                z = -body.getSizeZt() / 2.0;
                break;
            case BOX_BOTTOM:
                x = 0.0;
                y = -body.getSizeYt() / 2.0;
                z = 0.0;

                break;
            default:
                BDEBUG(TO_STRING(connection));
                assert(0);
        }
        transform.setOrigin(btVector3(x, y, z));
    }

    void Constraint::locateSphereConnection(const BodyPart& body, int connection, btTransform& transform) {
        btScalar x, y, z;
        switch (connection) {
            case SPHERE_TOP:
                x = 0.0;
                y = body.getSizeRt();
                z = 0.0;
                break;
                //First Ring from Top
            case SPHERE_R1_BACK:
                x = 0.0;
                y = body.getSizeRt() * sin(M_PI_4);
                z = -body.getSizeRt() * cos(M_PI_4);
                break;
            case SPHERE_R1_RIGHT:
                x = body.getSizeRt() * cos(M_PI_4);
                y = body.getSizeRt() * sin(M_PI_4);
                z = 0.0;
                break;
            case SPHERE_R1_FRONT:
                x = 0.0;
                y = body.getSizeRt() * sin(M_PI_4);
                z = body.getSizeRt() * cos(M_PI_4);
                break;
            case SPHERE_R1_LEFT:
                x = -body.getSizeRt() * cos(M_PI_4);
                y = body.getSizeRt() * sin(M_PI_4);
                z = 0.0;
                break;
                //Second Ring
            case SPHERE_R2_BACK_LEFT:
                x = -body.getSizeRt() / 2.0;
                y = body.getSizeRt() / 2.0;
                z = -body.getSizeRt() * cos(M_PI_4);
                break;
            case SPHERE_R2_BACK_RIGHT:
                x = body.getSizeRt() / 2.0;
                y = body.getSizeRt() / 2.0;
                z = -body.getSizeRt() * cos(M_PI_4);
                break;
            case SPHERE_R2_FRONT_RIGHT:
                x = body.getSizeRt() / 2.0;
                y = body.getSizeRt() / 2.0;
                z = body.getSizeRt() * cos(M_PI_4);
                break;
            case SPHERE_R2_FRONT_LEFT:
                x = -body.getSizeRt() / 2.0;
                y = body.getSizeRt() / 2.0;
                z = body.getSizeRt() * cos(M_PI_4);
                break;
                //Middle Ring
            case SPHERE_R3_BACK:
                x = 0.0;
                y = 0.0;
                z = -body.getSizeRt();
                break;
            case SPHERE_R3_RIGHT:
                x = body.getSizeRt();
                y = 0.0;
                z = 0.0;
                break;
            case SPHERE_R3_FRONT:
                x = 0.0;
                y = 0.0;
                z = body.getSizeRt();
                break;
            case SPHERE_R3_LEFT:
                x = -body.getSizeRt();
                y = 0.0;
                z = 0.0;
                break;
                //Fourth Ring
            case SPHERE_R4_BACK_LEFT:
                x = -body.getSizeRt() / 2.0;
                y = -body.getSizeRt() / 2.0;
                z = -body.getSizeRt() * cos(M_PI_4);
                break;
            case SPHERE_R4_BACK_RIGHT:
                x = body.getSizeRt() / 2.0;
                y = -body.getSizeRt() / 2.0;
                z = -body.getSizeRt() * cos(M_PI_4);
                break;
            case SPHERE_R4_FRONT_RIGHT:
                x = body.getSizeRt() / 2.0;
                y = -body.getSizeRt() / 2.0;
                z = body.getSizeRt() * cos(M_PI_4);
                break;
            case SPHERE_R4_FRONT_LEFT:
                x = -body.getSizeRt() / 2.0;
                y = -body.getSizeRt() / 2.0;
                z = body.getSizeRt() * cos(M_PI_4);
                break;
                //Fifth Ring
            case SPHERE_R5_BACK:
                x = 0.0;
                y = -body.getSizeRt() * sin(M_PI_4);
                z = -body.getSizeRt() * cos(M_PI_4);
                break;
            case SPHERE_R5_RIGHT:
                x = body.getSizeRt() * cos(M_PI_4);
                y = -body.getSizeRt() * sin(M_PI_4);
                z = 0.0;
                break;
            case SPHERE_R5_FRONT:
                x = 0.0;
                y = -body.getSizeRt() * sin(M_PI_4);
                z = body.getSizeRt() * cos(M_PI_4);
                break;
            case SPHERE_R5_LEFT:
                x = -body.getSizeRt() * cos(M_PI_4);
                y = -body.getSizeRt() * sin(M_PI_4);
                z = 0.0;
                break;
                //Bottom
            case SPHERE_BOTTOM:
                x = 0.0;
                y = -body.getSizeRt();
                z = 0.0;

                break;
            default:
                BDEBUG(TO_STRING(connection));
                assert(0);
        }
        transform.setOrigin(btVector3(x, y, z));
    }

    void Constraint::locateCylinderConnection(const BodyPart& body, int connection, int axis, btTransform& transform) {
        btScalar x, y, z;
        switch (connection) {
                //Top Cap
            case CYLINDER_TOP:
                x = 0.0;
                y = body.getSizeYt() / 2.0;
                z = 0.0;
                break;
                //Top Ring
            case CYLINDER_R1_BACK:
                x = 0.0;
                y = body.getSizeYt() / 2.0;
                z = -body.getSizeRt();
                break;
            case CYLINDER_R1_RIGHT:
                x = body.getSizeRt();
                y = body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            case CYLINDER_R1_FRONT:
                x = 0.0;
                y = body.getSizeYt() / 2.0;
                z = body.getSizeRt();
                break;
            case CYLINDER_R1_LEFT:
                x = -body.getSizeRt();
                y = body.getSizeYt() / 2.0;
                z = 0.0;
                break;
                //Second Ring
            case CYLINDER_R2_BACK:
                x = 0.0;
                y = body.getSizeYt() / 4.0;
                z = -body.getSizeRt();
                break;
            case CYLINDER_R2_RIGHT:
                x = body.getSizeRt();
                y = body.getSizeYt() / 4.0;
                z = 0.0;
                break;
            case CYLINDER_R2_FRONT:
                x = 0.0;
                y = body.getSizeYt() / 4.0;
                z = body.getSizeRt();
                break;
            case CYLINDER_R2_LEFT:
                x = -body.getSizeRt();
                y = body.getSizeYt() / 4.0;
                z = 0.0;
                break;
                //Middle Section
            case CYLINDER_R3_BACK:
                x = 0.0;
                y = 0.0;
                z = -body.getSizeRt();
                break;
            case CYLINDER_R3_RIGHT:
                x = body.getSizeRt();
                y = 0.0;
                z = 0.0;
                break;
            case CYLINDER_R3_FRONT:
                x = 0.0;
                y = 0.0;
                z = body.getSizeRt();
                break;
            case CYLINDER_R3_LEFT:
                x = -body.getSizeRt();
                y = 0.0;
                z = 0.0;
                break;
                //Fourth Ring
            case CYLINDER_R4_BACK:
                x = 0.0;
                y = -body.getSizeYt() / 4.0;
                z = -body.getSizeRt();
                break;
            case CYLINDER_R4_RIGHT:
                x = body.getSizeRt();
                y = -body.getSizeYt() / 4.0;
                z = 0.0;
                break;
            case CYLINDER_R4_FRONT:
                x = 0.0;
                y = -body.getSizeYt() / 4.0;
                z = body.getSizeRt();
                break;
            case CYLINDER_R4_LEFT:
                x = -body.getSizeRt();
                y = -body.getSizeYt() / 4.0;
                z = 0.0;
                break;
                //Bottom Side
            case CYLINDER_R5_BACK:
                x = 0.0;
                y = -body.getSizeYt() / 2.0;
                z = -body.getSizeRt();
                break;
            case CYLINDER_R5_RIGHT:
                x = body.getSizeRt();
                y = -body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            case CYLINDER_R5_FRONT:
                x = 0.0;
                y = -body.getSizeYt() / 2.0;
                z = body.getSizeRt();
                break;
            case CYLINDER_R5_LEFT:
                x = -body.getSizeRt();
                y = -body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            case CYLINDER_BOTTOM:
                x = 0.0;
                y = -body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            default:
                std::cout << connection << std::endl;
                assert(0);
        }
        switch (axis) {
            case 0: transform.setOrigin(btVector3(y, x, z));
                break;
            case 1: transform.setOrigin(btVector3(x, y, z));
                break;
            case 2: transform.setOrigin(btVector3(x, z, y));

                break;
            default:
                BDEBUG(TO_STRING(axis));
                assert(0);
        }
    }

    void Constraint::locateCapsuleConnection(const BodyPart& body, int connection, int axis, btTransform& transform) {
        btScalar x, y, z;
        switch (connection) {
                //Top Cap
            case CYLINDER_TOP:
                x = 0.0;
                y = (body.getSizeYt() / 2.0) + body.getSizeRt();
                z = 0.0;
                break;
            case CYLINDER_R1_BACK:
                x = 0.0;
                y = body.getSizeYt() / 2.0;
                z = -body.getSizeRt();
                break;
            case CYLINDER_R1_RIGHT:
                x = body.getSizeRt();
                y = body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            case CYLINDER_R1_FRONT:
                x = 0.0;
                y = body.getSizeYt() / 2.0;
                z = body.getSizeRt();
                break;
            case CYLINDER_R1_LEFT:
                x = -body.getSizeRt();
                y = body.getSizeYt() / 2.0;
                z = 0.0;
                break;
                //Second Ring
            case CYLINDER_R2_BACK:
                x = 0.0;
                y = body.getSizeYt() / 4.0;
                z = -body.getSizeRt();
                break;
            case CYLINDER_R2_RIGHT:
                x = body.getSizeRt();
                y = body.getSizeYt() / 4.0;
                z = 0.0;
                break;
            case CYLINDER_R2_FRONT:
                x = 0.0;
                y = body.getSizeYt() / 4.0;
                z = body.getSizeRt();
                break;
            case CYLINDER_R2_LEFT:
                x = -body.getSizeRt();
                y = body.getSizeYt() / 4.0;
                z = 0.0;
                break;
                //Middle Section
            case CYLINDER_R3_BACK:
                x = 0.0;
                y = 0.0;
                z = -body.getSizeRt();
                break;
            case CYLINDER_R3_RIGHT:
                x = body.getSizeRt();
                y = 0.0;
                z = 0.0;
                break;
            case CYLINDER_R3_FRONT:
                x = 0.0;
                y = 0.0;
                z = body.getSizeRt();
                break;
            case CYLINDER_R3_LEFT:
                x = -body.getSizeRt();
                y = 0.0;
                z = 0.0;
                break;
                //Fourth Ring
            case CYLINDER_R4_BACK:
                x = 0.0;
                y = -body.getSizeYt() / 4.0;
                z = -body.getSizeRt();
                break;
            case CYLINDER_R4_RIGHT:
                x = body.getSizeRt();
                y = -body.getSizeYt() / 4.0;
                z = 0.0;
                break;
            case CYLINDER_R4_FRONT:
                x = 0.0;
                y = -body.getSizeYt() / 4.0;
                z = body.getSizeRt();
                break;
            case CYLINDER_R4_LEFT:
                x = -body.getSizeRt();
                y = -body.getSizeYt() / 4.0;
                z = 0.0;
                break;
                //Bottom Side
            case CYLINDER_R5_BACK:
                x = 0.0;
                y = -body.getSizeYt() / 2.0;
                z = -body.getSizeRt();
                break;
            case CYLINDER_R5_RIGHT:
                x = body.getSizeRt();
                y = -body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            case CYLINDER_R5_FRONT:
                x = 0.0;
                y = -body.getSizeYt() / 2.0;
                z = body.getSizeRt();
                break;
            case CYLINDER_R5_LEFT:
                x = -body.getSizeRt();
                y = -body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            case CYLINDER_BOTTOM:
                x = 0.0;
                y = -(body.getSizeYt() / 2.0) - body.getSizeRt();
                z = 0.0;
                break;
            default:
                BDEBUG(TO_STRING(connection));
                assert(0);
        }
        switch (axis) {
            case 0: transform.setOrigin(btVector3(y, x, z));
                break;
            case 1: transform.setOrigin(btVector3(x, y, z));
                break;
            case 2: transform.setOrigin(btVector3(x, z, y));

                break;
            default:
                BDEBUG(TO_STRING(axis));
                assert(0);
        }
    }

    void Constraint::locateConeConnection(const BodyPart& body, int connection, int axis, btTransform& transform) {
        btScalar x, y, z;
        switch (connection) {
                //Top Cap
            case CONE_TOP:
                x = 0.0;
                y = (body.getSizeYt() / 2.0);
                z = 0.0;
                break;
                //Middle Section
            case CONE_MID_BACK:
                x = 0.0;
                y = 0.0;
                // A/a = B/a = H/h (Thales' Theorem) b = Ba/A
                z = -body.getSizeRt() * (body.getSizeYt() / 2.0) / body.getSizeYt();
                break;
            case CONE_MID_BACK_RIGHT:
                x = body.getSizeRt() * cos(M_PI_4) * (body.getSizeYt() / 2.0) / body.getSizeYt();
                y = 0.0;
                // A/a = B/a = H/h (Thales' Theorem) b = Ba/A
                z = -body.getSizeRt() * sin(M_PI_4) * (body.getSizeYt() / 2.0) / body.getSizeYt();
                break;
            case CONE_MID_RIGHT:
                // A/a = B/a = H/h (Thales' Theorem) b = Ba/A
                x = body.getSizeRt() * (body.getSizeYt() / 2.0) / body.getSizeYt();
                y = 0.0;
                z = 0.0;
                break;
            case CONE_MID_FRONT_RIGHT:
                x = body.getSizeRt() * cos(M_PI_4) * (body.getSizeYt() / 2.0) / body.getSizeYt();
                y = 0.0;
                // A/a = B/a = H/h (Thales' Theorem) b = Ba/A
                z = body.getSizeRt() * sin(M_PI_4) * (body.getSizeYt() / 2.0) / body.getSizeYt();
                break;
            case CONE_MID_FRONT:
                x = 0.0;
                y = 0.0;
                // A/a = B/a = H/h (Thales' Theorem) b = Ba/A
                z = body.getSizeRt() * (body.getSizeYt() / 2.0) / body.getSizeYt();
                break;
            case CONE_MID_FRONT_LEFT:
                x = -body.getSizeRt() * cos(M_PI_4) * (body.getSizeYt() / 2.0) / body.getSizeYt();
                y = 0.0;
                // A/a = B/a = H/h (Thales' Theorem) b = Ba/A
                z = body.getSizeRt() * sin(M_PI_4) * (body.getSizeYt() / 2.0) / body.getSizeYt();
                break;
            case CONE_MID_LEFT:
                // A/a = B/a = H/h (Thales' Theorem) b = Ba/A
                x = -body.getSizeRt() * (body.getSizeYt() / 2.0) / body.getSizeYt();
                y = 0.0;
                z = 0.0;
                break;
            case CONE_MID_BACK_LEFT:
                // A/a = B/a = H/h (Thales' Theorem) b = Ba/A
                x = -body.getSizeRt() * cos(M_PI_4) * (body.getSizeYt() / 2.0) / body.getSizeYt();
                y = 0.0;
                z = -body.getSizeRt() * sin(M_PI_4) * (body.getSizeYt() / 2.0) / body.getSizeYt();
                break;
                //Bottom Section
            case CONE_BOT_BACK:
                x = 0.0;
                y = -body.getSizeYt() / 2.0;
                z = -body.getSizeRt();
                break;
            case CONE_BOT_BACK_RIGHT:
                x = body.getSizeRt() * cos(M_PI_4);
                y = -body.getSizeYt() / 2.0;
                z = -body.getSizeRt() * sin(M_PI_4);
                break;
            case CONE_BOT_RIGHT:
                x = body.getSizeRt();
                y = -body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            case CONE_BOT_FRONT_RIGHT:
                x = body.getSizeRt() * cos(M_PI_4);
                y = -body.getSizeYt() / 2.0;
                z = body.getSizeRt() * sin(M_PI_4);
                break;
            case CONE_BOT_FRONT:
                x = 0.0;
                y = -body.getSizeYt() / 2.0;
                z = body.getSizeRt();
                break;
            case CONE_BOT_FRONT_LEFT:
                x = -body.getSizeRt() * cos(M_PI_4);
                y = -body.getSizeYt() / 2.0;
                z = body.getSizeRt() * sin(M_PI_4);
                break;
            case CONE_BOT_LEFT:
                x = -body.getSizeRt();
                y = -body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            case CONE_BOT_BACK_LEFT:
                x = -body.getSizeRt() * cos(M_PI_4);
                y = -body.getSizeYt() / 2.0;
                z = -body.getSizeRt() * sin(M_PI_4);
                break;
            case CONE_BOT_BACK_LEFT_2:
                x = -body.getSizeRt() / 2.0 * cos(M_PI_4);
                y = -body.getSizeYt() / 2.0;
                z = -body.getSizeRt() / 2.0 * sin(M_PI_4);
                break;
            case CONE_BOT_BACK_RIGHT_2:
                x = body.getSizeRt() / 2.0 * cos(M_PI_4);
                y = -body.getSizeYt() / 2.0;
                z = -body.getSizeRt() / 2.0 * sin(M_PI_4);
                break;
            case CONE_BOT_FRONT_RIGHT_2:
                x = body.getSizeRt() / 2.0 * cos(M_PI_4);
                y = -body.getSizeYt() / 2.0;
                z = body.getSizeRt() / 2.0 * sin(M_PI_4);
                break;
            case CONE_BOT_FRONT_LEFT_2:
                x = -body.getSizeRt() / 2.0 * cos(M_PI_4);
                y = -body.getSizeYt() / 2.0;
                z = body.getSizeRt() / 2.0 * sin(M_PI_4);
                break;
            case CONE_BOTTOM:
                x = 0.0;
                y = -body.getSizeYt() / 2.0;
                z = 0.0;
                break;
            default:
                BDEBUG(TO_STRING(connection));
                assert(0);
        }
        switch (axis) {
            case 0: transform.setOrigin(btVector3(y, x, z));
                break;
            case 1: transform.setOrigin(btVector3(x, y, z));
                break;
            case 2: transform.setOrigin(btVector3(x, z, y));

                break;
            default:
                BDEBUG(TO_STRING(axis));
                assert(0);
        }
    }


    //-----------------------------------------------------------
    //XML
    //-----------------------------------------------------------

    Constraint* Constraint::fromXml(const QDomElement& xml, bool* ok) {
        Constraint* result = new Constraint();
        result->setId(xml.attribute("id", "0").toInt(ok));
        result->setName(xml.attribute("name", "").toStdString());
        result->setType((eConstraintType) xml.attribute("type", QString(CT_HINGE)).toInt(ok));
        result->setIdBodyA(xml.attribute("id-body-a", "0").toInt(ok));
        result->setConnectionA(xml.attribute("connection-a", "0").toInt(ok));
        result->setIdBodyB(xml.attribute("id-body-b", "0").toInt(ok));
        result->setConnectionB(xml.attribute("connection-b", "0").toInt(ok));
        result->setConeSwingX(xml.attribute("cone-swing-x", "0.0").toDouble(ok));
        result->setConeTwistY(xml.attribute("cone-twist-y", "0.0").toDouble(ok));
        result->setConeSwingZ(xml.attribute("cone-swing-z", "0.0").toDouble(ok));

        //        result->setHingeLowerLimit(xml.attribute("hinge-lower-limit", "0.0").toDouble(ok));
        //        result->setHingeUpperLimit(xml.attribute("hinge-upper-limit", "0.0").toDouble(ok));
        //
        //        result->setAngularLowerLimits(btVector3(
        //                xml.attribute("angular-lower-limit-x", "0.0").toDouble(ok),
        //                xml.attribute("angular-lower-limit-y", "0.0").toDouble(ok),
        //                xml.attribute("angular-lower-limit-z", "0.0").toDouble(ok)));
        //        result->setAngularUpperLimits(btVector3(
        //                xml.attribute("angular-upper-limit-x", "0.0").toDouble(ok),
        //                xml.attribute("angular-upper-limit-y", "0.0").toDouble(ok),
        //                xml.attribute("angular-upper-limit-z", "0.0").toDouble(ok)));
        //
        //        result->setLinearLowerLimits(btVector3(
        //                xml.attribute("linear-lower-limit-x", "0.0").toDouble(ok),
        //                xml.attribute("linear-lower-limit-y", "0.0").toDouble(ok),
        //                xml.attribute("linear-lower-limit-z", "0.0").toDouble(ok)));
        //        result->setLinearUpperLimits(btVector3(
        //                xml.attribute("linear-upper-limit-x", "0.0").toDouble(ok),
        //                xml.attribute("linear-upper-limit-y", "0.0").toDouble(ok),
        //                xml.attribute("linear-upper-limit-z", "0.0").toDouble(ok)));
        if (!*ok) {

            BDEBUG("Error in Constraint::fromXml");
        }
        return result;
    }

    void Constraint::toXml(QDomDocument& xml, QDomElement* parent) const {
        QDomElement constraint = xml.createElement("Constraint");

        constraint.setAttribute("id", getId());
        constraint.setAttribute("name", QString(getName().c_str()));
        constraint.setAttribute("type", getType());
        constraint.setAttribute("id-body-a", getIdBodyA());
        constraint.setAttribute("connection-a", getConnectionA());
        constraint.setAttribute("id-body-b", getIdBodyB());
        constraint.setAttribute("connection-b", getConnectionB());

        constraint.setAttribute("cone-swing-x", getConeSwingX());
        constraint.setAttribute("cone-twist-y", getConeTwistY());
        constraint.setAttribute("cone-swing-z", getConeSwingZ());
        //                constraint.setAttribute("hinge-lower-limit", getHingeLowerLimit());
        //                constraint.setAttribute("hinge-upper-limit", getHingeUpperLimit());
        //
        //                constraint.setAttribute("angular-lower-limit-x", getAngularLowerLimits().x());
        //                constraint.setAttribute("angular-lower-limit-y", getAngularLowerLimits().y());
        //                constraint.setAttribute("angular-lower-limit-z", getAngularLowerLimits().z());
        //                constraint.setAttribute("angular-upper-limit-x", getAngularUpperLimits().x());
        //                constraint.setAttribute("angular-upper-limit-y", getAngularUpperLimits().y());
        //                constraint.setAttribute("angular-upper-limit-z", getAngularUpperLimits().z());
        //
        //                constraint.setAttribute("linear-lower-limit-x", getLinearLowerLimits().x());
        //                constraint.setAttribute("linear-lower-limit-y", getLinearLowerLimits().y());
        //                constraint.setAttribute("linear-lower-limit-z", getLinearLowerLimits().z());
        //                constraint.setAttribute("linear-upper-limit-x", getLinearUpperLimits().x());
        //                constraint.setAttribute("linear-upper-limit-y", getLinearUpperLimits().y());
        //                constraint.setAttribute("linear-upper-limit-z", getLinearUpperLimits().z());

        if (parent) {
            parent->appendChild(constraint);
        } else {

            xml.appendChild(constraint);
        }
    }

    std::string Constraint::toXml() const {
        QDomDocument doc;
        this->toXml(doc);
        return doc.toString().toStdString();
    }





}
